#include "rtps/storages/ThreadSafeCircularBuffer.h"

#if TSCB_VERBOSE && RTPS_GLOBAL_VERBOSE
#define TSCB_LOG(...)                                                          \
  if (true) {                                                                  \
    printf("[TSCircularBuffer] ");                                             \
    printf(__VA_ARGS__);                                                       \
    printf("\n");                                                              \
  }
#else
#define TSCB_LOG(...) //
#endif

static inline void
_tscb_incrementIterator(struct rtps_ThreadSafeCircularBuffer *tscb, uint16_t *iterator)
{
    ++(*iterator);
    if ((*iterator) >= (sizeof(tscb->m_buffer)/sizeof(tscb->m_buffer[0]))) {
        *iterator = 0;
    }
}

static inline void
_tscb_incrementTail(struct rtps_ThreadSafeCircularBuffer *tscb)
{
    _tscb_incrementIterator(tscb, &(tscb->m_tail));
}

static inline void
_tscb_incrementHead(struct rtps_ThreadSafeCircularBuffer *tscb)
{
    _tscb_incrementIterator(tscb, &(tscb->m_head));
    if (tscb->m_head == tscb->m_tail) {
        _tscb_incrementTail(tscb);
    }
}

static bool
_tscb_isFull(struct rtps_ThreadSafeCircularBuffer *tscb)
{
    uint16_t it = tscb->m_head;
    
    _tscb_incrementIterator(tscb, &it);
    
    if (it == tscb->m_tail) {
        return true;
    }
    
    return false;
}

struct rtps_ThreadSafeCircularBuffer *
rtps_ThreadSafeCircularBuffer_init(void (*args)(void *))
{
    struct rtps_ThreadSafeCircularBuffer *tscb =
        (struct rtps_ThreadSafeCircularBuffer *) calloc(1, sizeof(struct rtps_ThreadSafeCircularBuffer));

    if (tscb == NULL)
        return NULL;

    tscb->decallback = args;
    if (sys_mutex_new(&(tscb->m_mutex)) != ERR_OK) {
        TSCB_LOG("Failed to create mutex \n");
        free(tscb);
        return NULL;
    } else {
        TSCB_LOG("Successfully created mutex at %p\n",
                (void *)(&(tscb->m_mutex)));
        tscb->m_initialized = true;
        return tscb;
    }
}

void
rtps_ThreadSafeCircularBuffer_deinit(struct rtps_ThreadSafeCircularBuffer *tscb)
{
    if (tscb->m_initialized) {
        //clear queues
        rtps_ThreadSafeCircularBuffer_clear(tscb);
        sys_mutex_free(&(tscb->m_mutex));
    }
    free(tscb);
}

bool
rtps_ThreadSafeCircularBuffe_moveElementIntoBuffer(
    struct rtps_ThreadSafeCircularBuffer *tscb,
    void *elem)
{
    rtps_Lock(&tscb->m_mutex);

    if (!_tscb_isFull(tscb)) {
        tscb->m_buffer[tscb->m_head] = elem;
        _tscb_incrementHead(tscb);
        rtps_Unlock(&tscb->m_mutex);
        return true;
    } else {
        rtps_Unlock(&tscb->m_mutex);
        return false;
    }
}

bool
rtps_ThreadSafeCircularBuffer_moveFirstInto(
    struct rtps_ThreadSafeCircularBuffer *tscb,
    void **hull)
{
    rtps_Lock(&tscb->m_mutex);
    if (tscb->m_head != tscb->m_tail) {
        *hull = tscb->m_buffer[tscb->m_tail];
        _tscb_incrementTail(tscb);
        rtps_Unlock(&tscb->m_mutex);
        return true;
    } else {
        rtps_Unlock(&tscb->m_mutex);
        return false;
    }
}

void
rtps_ThreadSafeCircularBuffer_clear(struct rtps_ThreadSafeCircularBuffer *tscb)
{
    void *hull = NULL;

    rtps_Lock(&tscb->m_mutex);
    while(rtps_ThreadSafeCircularBuffer_moveFirstInto(tscb, &hull)) {
        tscb->decallback(hull);
    }
    tscb->m_head = tscb->m_tail;
    rtps_Unlock(&tscb->m_mutex);
}
